#ifndef ROTATEDBOX_H
#define ROTATEDBOX_H

#include "Core/Vec.h"
#include "Core/numlib.h"
#include <algorithm>

template <int Dim>
class RotatedBox
{
public:
  using rVec = Vec<Real, Dim>;

  RotatedBox(const rVec &aSides, const rVec *aBasis)
      : sides(aSides)
  {
    for(int d = 0; d < Dim; ++d)
      basis[d] = aBasis[d];
  }

  rVec getKsi(const rVec &x) const
  {
    rVec ksi;
    for(int d = 0; d < Dim; ++d)
      ksi[d] = dot(x, basis[d]);
    return ksi;
  }

  rVec getX(const rVec &ksi) const
  {
    rVec x(0.0);
    for(int d = 0; d < Dim; ++d)
      x = x + basis[d] * ksi[d];
    return x;
  }

  void check(const rVec &q, rVec &closestPoint, rVec &normal, bool &contained, Real tol) const
  {
    rVec ksi = getKsi(q);
    contained = true;
    for(int d = 0; d < Dim; ++d) {
      if(ksi[d] < 0.0) {
        ksi[d] = 0.0;
        contained = false;
      } else if(ksi[d] > sides[d]) {
        ksi[d] = sides[d];
        contained = false;
      }
    }
    Real dist[2*Dim];
    for(int d = 0; d < Dim; ++d) {
      dist[2*d+0] = ksi[d] - 0.0;
      dist[2*d+1] = sides[d] - ksi[d];
    }
    int m = std::min_element(&dist[0], &dist[2*Dim]) - dist;
    closestPoint = ksi;
    if(m % 2 == 0)
      closestPoint[m/2] = 0.0;
    else
      closestPoint[m/2] = sides[m/2];
    closestPoint = getX(closestPoint);
    normal = normalize(closestPoint - q);
    if(!contained)
      normal = -normal;
  }

protected:
  rVec sides;
  rVec basis[Dim];
};

#endif //ROTATEDBOX_H
